// Modified by: mzy at 2024-04-30

#ifndef PID_HPP
#define PID_HPP

#include <vector>
#include <eigen3/Eigen/Dense>

using namespace std;

template <typename T>
class CircularBuffer
{
private:
  std::vector<T> buffer;
  size_t head{0};
  size_t count{0};
  size_t capacity{0};

public:
  CircularBuffer(size_t size) : capacity(size), buffer(size) {}

  void push(const T &value)
  {
    buffer[head] = value;
    head = (head + 1) % capacity; // 循环索引
    if (count < capacity) count++;
  }

  // TODO: check index is valid
  T back(size_t index = 0) const
  {
    return buffer[(head + capacity - index - 1) % capacity];
  }

  size_t size() const
  {
    return count;
  }
};

class PID
{
public:
  PID() {}
  ~PID() {}

private:
  double Kp_{1.0};
  double Ki_{0.0};
  double Kd_{0.0};

  double P_Out_{0.0};  // P环节输出
  double I_Out_{0.0};  // I环节输出
  double D_Out_{0.0};  // D环节输出
  double Output_{0.0}; // PID输出

  double Imax_{0.0};       // 积分上限
  double Output_max_{0.0}; // 输出最大值
  double errThres{0.0};    // 误差死区(if error_<errThres, error_<0)

  double error_{0.0};       // 误差量 = 实际值 - 期望值
  double delta_time_{0.05}; // 时间间隔dt

  bool start_intergrate_flag{false}; // 是否积分标志[进入offboard(启控)后,才开始积分] 1 or 0

  int max_error_list_size_{10}; // 误差表最大长度
  // 误差表,用作计算微分项 平滑窗口 [1st time, 2nd data]
  CircularBuffer<std::pair<double, double>> error_list_{10};

  /**
   * @brief saturation function for the PID controller
   */
  double satfunc(double data, double Max, double Thres)
  {
    if (fabs(data) < Thres) return 0;

    if (fabs(data) > Max) return (data > 0) ? Max : -Max;

    return data;
  }

public:
  double getOutput() const { return Output_; }

  void setPID(double p, double i, double d)
  {
    Kp_ = p;
    Ki_ = i;
    Kd_ = d;
  }

  /**
   * @brief set the saturation of the PID controller
   *
   * @param i_max   the maximum value of the integral term
   * @param total_max the maximum value of the output
   * @param thres   the threshold of the error_
   */
  void setLimit(double i_max, double total_max, double thres)
  {
    Imax_ = i_max;
    Output_max_ = total_max;
    errThres = thres;
  }

  void setIntergrateFlag(bool flag)
  {
    start_intergrate_flag = flag;
  }

  // 输入误差 和 当前时间
  void add_error(double error, double curtime)
  {
    error_ = error;

    if (error_list_.size() == 0)
      delta_time_ = 0.05;
    else
      delta_time_ = curtime - error_list_.back().first;

    error_list_.push(make_pair(curtime, error_));
  }

  double calculate()
  {
    P_Out_ = Kp_ * error_; // P环节输出值

    if (start_intergrate_flag && Ki_ != 0) {
      I_Out_ = I_Out_ + Ki_ * error_ * delta_time_; // I环节输出值
      I_Out_ = satfunc(I_Out_, Imax_, 0);           // I环节限幅[I_Out_<=Imax_]
    }
    else {
      I_Out_ = 0;
    }

    if (error_list_.size() > 3 && Kd_ != 0) {
      D_Out_ = Kd_ * (error_list_.back().second - error_list_.back(1).second) /
               delta_time_;
    }
    else {
      D_Out_ = 0;
    }

    Output_ = satfunc(P_Out_ + I_Out_ + D_Out_, Output_max_, errThres);

    return Output_;
  }
};

class PID3
{
public:
  PID3() {}
  ~PID3() {}

  bool setPID(const std::vector<double> &p,
              const std::vector<double> &i,
              const std::vector<double> &d)
  {
    if (p.size() != 3 || i.size() != 3 || d.size() != 3)
      return false;

    pid_x_.setPID(p[0], i[0], d[0]);
    pid_y_.setPID(p[1], i[1], d[1]);
    pid_z_.setPID(p[2], i[2], d[2]);

    return true;
  }

  void setLimit(const Eigen::Vector3d &i_max,
                const Eigen::Vector3d &total_max,
                const Eigen::Vector3d &thres)
  {
    pid_x_.setLimit(i_max(0), total_max(0), thres(0));
    pid_y_.setLimit(i_max(1), total_max(1), thres(1));
    pid_z_.setLimit(i_max(2), total_max(2), thres(2));
  }

  void setIntergrateFlag(bool flag)
  {
    pid_x_.setIntergrateFlag(flag);
    pid_y_.setIntergrateFlag(flag);
    pid_z_.setIntergrateFlag(flag);
  }

  void add_error(const Eigen::Vector3d &error, double curtime)
  {
    pid_x_.add_error(error(0), curtime);
    pid_y_.add_error(error(1), curtime);
    pid_z_.add_error(error(2), curtime);
  }

  Eigen::Vector3d calculate()
  {
    return Eigen::Vector3d(pid_x_.calculate(),
                           pid_y_.calculate(),
                           pid_z_.calculate());
  }

  Eigen::Vector3d getOutput() const
  {
    return Eigen::Vector3d(pid_x_.getOutput(),
                           pid_y_.getOutput(),
                           pid_z_.getOutput());
  }

private:
  PID pid_x_;
  PID pid_y_;
  PID pid_z_;
};

#endif // PID_HPP
